
#include "ElementBase.h"
#include "MyTime.h"
#include "BasisFunctionsLagrange.h"
#include "GeomDataLagrange.h"
#include "QuadratureUtil.h"
#include "utilitiesmaterial.h"
#include "TimeFunction.h"

extern  std::vector<unique_ptr<TimeFunction> > timeFunction;
extern MyTime           myTime;

using namespace std;

int ElementBase::elemcount = 0;


ElementBase::ElementBase()
{
//  cout << "     ElementBase: constructr ...\n\n";

  elenum = elemcount++;

  ndof = nsize = nivGP = nGP = elenum = 0;
  subdomId = 0;
  ELEM_FORMULATION = "Displacement";

  elmDat   = NULL;
  matDat   = NULL;
}





ElementBase::~ElementBase()
{
  if (elmDat!=NULL) elmDat = NULL;
  if (matDat!=NULL) matDat = NULL;

//  cout << "     ElementBase: destructor ...\n\n";
}




void ElementBase::prepareElemData()
{
    assert(nodeNums.size() == npElem);

    vals2project.resize(npElem);

    nsize = npElem*ndof;

    globalDOFnums.resize(nsize);

    int ii, jj, kk=0, ind;
    for(ii=0; ii<nodeNums.size(); ii++)
    {
      ind = nodeNums[ii]*ndof;
      for(jj=0;jj<ndof;jj++)
        globalDOFnums[kk++] = ind+jj;
    }

    // set internal variables
    //initialiseIntVar();

    return;
}




void ElementBase::setnivGP()
{

    return;
}




void ElementBase::initialiseIntVar()
{
    // set nivGP value
    switch(MatlData->getMaterialTypeNameNumber())
    {
        case 101:
        case 103:
        case 104:
        case 105:
        case 106:
        case 2006:
        case 2007:

            nivGP = (int) MatlData->data_viscoelastic[0];

        break;

        default:
            nivGP = 0;
        break;
    }

    // allocate memory for internal variables
    if(nivGP > 0)
    {
        ivar.initialise(nivGP*9, nGP);
    }

    return;
}




void  ElementBase::computeDefGrad2D(VectorXd& dN_dx, VectorXd& dN_dy, MatrixXd& F)
{
    double  ux, uy;
    F = Matrix3d::Identity();

    for(int ii=0;ii<npElem;ii++)
    {
        int ind = nodeNums[ii]*ndof;

        ux = SolnData->disp[ind];
        uy = SolnData->disp[ind+1];

        F(0,0) += (ux*dN_dx[ii]);
        F(0,1) += (ux*dN_dy[ii]);

        F(1,0) += (uy*dN_dx[ii]);
        F(1,1) += (uy*dN_dy[ii]);
    }

    return;
}




void  ElementBase::computeDefGrad2DPrev(VectorXd& dN_dx, VectorXd& dN_dy, MatrixXd& F)
{
    double  ux, uy;
    F = Matrix3d::Identity();

    for(int ii=0;ii<npElem;ii++)
    {
        int ind = nodeNums[ii]*ndof;

        ux = SolnData->dispPrev[ind];
        uy = SolnData->dispPrev[ind+1];

        F(0,0) += (ux*dN_dx[ii]);
        F(0,1) += (ux*dN_dy[ii]);

        F(1,0) += (uy*dN_dx[ii]);
        F(1,1) += (uy*dN_dy[ii]);
    }

    return;
}




void  ElementBase::computeDefGrad2DCur(VectorXd& dN_dx, VectorXd& dN_dy, MatrixXd& F)
{
    double  ux, uy;
    F = Matrix3d::Identity();

    for(int ii=0;ii<npElem;ii++)
    {
        int ind = nodeNums[ii]*ndof;

        ux = SolnData->dispCur[ind];
        uy = SolnData->dispCur[ind+1];

        F(0,0) += (ux*dN_dx[ii]);
        F(0,1) += (ux*dN_dy[ii]);

        F(1,0) += (uy*dN_dx[ii]);
        F(1,1) += (uy*dN_dy[ii]);
    }

    return;
}



void  ElementBase::computeDefGrad(VectorXd& dN_dx, VectorXd& dN_dy, VectorXd& dN_dz, MatrixXd& F)
{
    double  ux, uy, uz;
    F = Matrix3d::Identity();

    for(int ii=0;ii<npElem;ii++)
    {
        int ind = nodeNums[ii]*ndof;

        ux = SolnData->disp[ind];
        uy = SolnData->disp[ind+1];
        uz = SolnData->disp[ind+2];

        F(0,0) += (ux*dN_dx[ii]);
        F(0,1) += (ux*dN_dy[ii]);
        F(0,2) += (ux*dN_dz[ii]);

        F(1,0) += (uy*dN_dx[ii]);
        F(1,1) += (uy*dN_dy[ii]);
        F(1,2) += (uy*dN_dz[ii]);

        F(2,0) += (uz*dN_dx[ii]);
        F(2,1) += (uz*dN_dy[ii]);
        F(2,2) += (uz*dN_dz[ii]);
    }

    return;
}




void  ElementBase::computeDefGradPrev(VectorXd& dN_dx, VectorXd& dN_dy, VectorXd& dN_dz, MatrixXd& F)
{
    double  ux, uy, uz;
    F = Matrix3d::Identity();

    for(int ii=0;ii<npElem;ii++)
    {
        int ind = nodeNums[ii]*ndof;

        ux = SolnData->dispPrev[ind];
        uy = SolnData->dispPrev[ind+1];
        uz = SolnData->dispPrev[ind+2];

        F(0,0) += (ux*dN_dx[ii]);
        F(0,1) += (ux*dN_dy[ii]);
        F(0,2) += (ux*dN_dz[ii]);

        F(1,0) += (uy*dN_dx[ii]);
        F(1,1) += (uy*dN_dy[ii]);
        F(1,2) += (uy*dN_dz[ii]);

        F(2,0) += (uz*dN_dx[ii]);
        F(2,1) += (uz*dN_dy[ii]);
        F(2,2) += (uz*dN_dz[ii]);
    }

    return;
}




void  ElementBase::computeDefGradCur(VectorXd& dN_dx, VectorXd& dN_dy, VectorXd& dN_dz, MatrixXd& F)
{
    double  ux, uy, uz;
    F = Matrix3d::Identity();

    for(int ii=0;ii<npElem;ii++)
    {
        int ind = nodeNums[ii]*ndof;

        ux = SolnData->dispCur[ind];
        uy = SolnData->dispCur[ind+1];
        uz = SolnData->dispCur[ind+2];

        F(0,0) += (ux*dN_dx[ii]);
        F(0,1) += (ux*dN_dy[ii]);
        F(0,2) += (ux*dN_dz[ii]);

        F(1,0) += (uy*dN_dx[ii]);
        F(1,1) += (uy*dN_dy[ii]);
        F(1,2) += (uy*dN_dz[ii]);

        F(2,0) += (uz*dN_dx[ii]);
        F(2,1) += (uz*dN_dy[ii]);
        F(2,2) += (uz*dN_dz[ii]);
    }

    return;
}





int  ElementBase::applyDirichletBCs(MatrixXd& Klocal, VectorXd& Flocal)
{
  // add contributions to the rhs vector
  // from nodes with specified displacement BCs

  int aa, bb, ii, jj;
  double fact=0.0;

  for(ii=0; ii<nsize; ii++)
  {
    aa = forAssyVec[ii];
    if(aa == -1) // this DOF has a prescibed value
    {
      fact = SolnData->dispApplied[globalDOFnums[ii]];

      for(jj=0; jj<nsize; jj++)
      {
        bb = forAssyVec[jj];
        if( bb != -1 )
        {
          Flocal(jj) -= Klocal(jj, ii) * fact;
        }
      }
    }
  }

  return 0;
}





int  ElementBase::applyDirichletBCsMixed(int var2_offset, MatrixXd& Kuu, MatrixXd& Kup, MatrixXd& Kpu, MatrixXd& Kpp, VectorXd& Flocal1, VectorXd& Flocal2)
{
  // add contributions to the rhs vector
  // from nodes with specified displacement BCs

  int aa, bb, ii, jj, sizeP = forAssyVecPres.size();
  double fact=0.0;

  if(var2_offset == 0) // for the explicit scheme
  {
    // applied displacement (velocity) dof for the solid (fluid)
    for(ii=0; ii<nsize; ii++)
    {
      aa = forAssyVec[ii];
      if(aa == -1) // this DOF has a prescibed value
      {
        fact = SolnData->dispApplied[globalDOFnums[ii]];

        for(jj=0; jj<sizeP; jj++)
        {
          //cout << Flocal2(jj) << endl;
          //cout << ii << '\t' << aa << '\t' << globalDOFnums[ii] << '\t' << jj << '\t' << forAssyVecPres[jj] << endl;
          //cout << Flocal2(jj) << '\t' << Kpu(jj, ii) << endl;
          if( forAssyVecPres[jj] != -1 )
          {
            Flocal2(jj) -= Kpu(jj, ii) * fact;
          }
        }
      }
    }

    // applied pressure
    for(ii=0; ii<sizeP; ii++)
    {
      aa = forAssyVecPres[ii];
      if(aa == -1) // this DOF has a prescibed value
      {
        fact = SolnData->presApplied[nodeNums[ii]];

        for(jj=0; jj<nsize; jj++)
        {
          if( forAssyVec[jj] != -1 )
          {
            Flocal1(jj) -= Kup(jj, ii) * fact;
          }
        }

        for(jj=0; jj<sizeP; jj++)
        {
          if( forAssyVecPres[jj] != -1 )
          {
            Flocal2(jj) -= Kpp(jj, ii) * fact;
          }
        }
      }
    }
  }
  else  // for the implicit scheme
  {
    // applied displacement (velocity) dof for the solid (fluid)
    for(ii=0; ii<nsize; ii++)
    {
      aa = forAssyVec[ii];
      if(aa == -1) // this DOF has a prescibed value
      {
        fact = SolnData->dispApplied[globalDOFnums[ii]];

        for(jj=0; jj<nsize; jj++)
        {
          if( forAssyVec[jj] != -1 )
          {
            Flocal1(jj) -= Kuu(jj, ii) * fact;
          }
        }

        for(jj=0; jj<sizeP; jj++)
        {
          if( forAssyVecPres[jj] != -1 )
          {
            Flocal2(jj) -= Kpu(jj, ii) * fact;
          }
        }
      }
    }

    // applied pressure
    for(ii=0; ii<sizeP; ii++)
    {
      aa = forAssyVecPres[ii];
      if(aa == -1) // this DOF has a prescibed value
      {
        fact = SolnData->presApplied[nodeNums[ii]];

        for(jj=0; jj<nsize; jj++)
        {
          if( forAssyVec[jj] != -1 )
          {
            Flocal1(jj) -= Kup(jj, ii) * fact;
          }
        }

        for(jj=0; jj<sizeP; jj++)
        {
          if( forAssyVecPres[jj] != -1 )
          {
            Flocal2(jj) -= Kpp(jj, ii) * fact;
          }
        }
      }
    }
  }

  return 0;
}



int  ElementBase::applyDirichletBCs2field(int schemeType, MatrixXd& K11, MatrixXd& K12, MatrixXd& K21, MatrixXd& K22, VectorXd& Flocal1, VectorXd& Flocal2, vector<PetscInt>& forAssyVar1, vector<int>& forAssyVar2, VectorXd& dispApplied, VectorXd& presApplied)
{
  // add contributions to the rhs vector
  // from nodes with specified displacement BCs

  int aa, bb, ii, jj, size1 = forAssyVar1.size(), size2 = forAssyVar2.size();
  double fact=0.0;

  //printVector(globalDOFnums);

    // applied displacement dof for the solid
    for(ii=0; ii<size1; ii++)
    {
      aa = forAssyVar1[ii];
      if(aa == -1) // this DOF has a prescibed value
      {
        fact = dispApplied[globalDOFnums[ii]];

        for(jj=0; jj<size1; jj++)
        {
          if( forAssyVar1[jj] != -1 )
          {
            Flocal1(jj) -= K11(jj, ii) * fact;
          }
        }

        for(jj=0; jj<size2; jj++)
        {
          if( forAssyVar2[jj] != -1 )
          {
            Flocal2(jj) -= K21(jj, ii) * fact;
          }
        }
      }
    }

    // applied pressure
    for(ii=0; ii<size2; ii++)
    {
      aa = forAssyVar2[ii];
      if(aa == -1) // this DOF has a prescribed value
      {
        fact = presApplied[nodeNums[ii]];

        //cout <<  ii <<  '\t' <<  fact <<  endl;

        for(jj=0; jj<size1; jj++)
        {
          if( forAssyVar1[jj] != -1 )
          {
            Flocal1(jj) -= K12(jj, ii) * fact;
          }
        }

        for(jj=0; jj<size2; jj++)
        {
          if( forAssyVar2[jj] != -1 )
          {
            Flocal2(jj) -= K22(jj, ii) * fact;
          }
        }
      }
    }

    return 0;
}




void ElementBase::printStiffnessMatrix()
{
/*
  int ii, jj;
   printf(" Stiffness Matrix for element : %5d\n\n",elenum);
   for(ii=0;ii<nsize;ii++)
   {
      for(jj=0;jj<nsize;jj++)
      {
         printf("%12.6f\t", Klocal(ii,jj)) ;
      }
      printf("\n");
   }
   printf("\n");
   printf("\n");
*/

  return;
}




void ElementBase::printForceVector()
{
   printf(" Residue Vector for element : %5d\n\n",elenum);
   for(int ii=0;ii<nsize;ii++)
   {
     printf("\t%5d\t%12.8f\n",ii,resi[ii]);
   }
   printf("\n");
   printf("\n");

  return;
}



void ElementBase::printPrimVariable()
{
     int index, ii, dof;
     for(ii=0;ii<npElem;ii++)
     {
        index = ndof*ii;
        cout << '\t' << ii << '\t' ;
        for(dof=0;dof<ndof;dof++)
           cout << primvar[index+dof] << '\t' ;
        cout << endl;
     }

  return;
}


/*
int  ElementBase::calcError(int index)
{
    //cout << " Error ... " << endl;

    // computing error for Navier-Stokes
    ///////////////////////////////////////////////////////////
    
    //Stokes2DEx1  analy;
    //Stokes2DEx2  analy;

    //Stokes2DEx3  analy;
    //Kovasznay  analy;
    //analy.SetPressure(0.0);

    //PearsonVortex analy;

    elmDat = &(SolnData->ElemProp[elmType].data[0]);
    //matDat = &(SolnData->MatlProp[matType].data[0]);

    double rho = elmDat[4];
    double mu  = elmDat[5];

    KimMoinFlow  analy(rho, mu, 1.0);
    //Kovasznay  analy;

    int      ii, jj, gp1, gp2, TI;
    double   Jac, dvol, diff, fact, val;
    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem);
    double  param[2], geom[2];

    elemError = 0.0;
    



 if(index < 3) // L2 norm in x-velocity (index=0), y-velocity (index=1) and pressure (index=2)
 {
    for(gp2=0;gp2<nGP;gp2++)
    {
        param[1] = GeomData->gausspoints2[gp2];

    for(gp1=0;gp1<nGP;gp1++)
    {
          param[0] = GeomData->gausspoints1[gp1];

          GeomData->computeBasisFunctions2D(0, 2, degree, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);

          dvol = GeomData->gaussweights2[gp2] * GeomData->gaussweights1[gp1] * Jac;

          geom[0] = computeGeomOrig(0, N);
          geom[1] = computeGeomOrig(1, N);

          val = analy.computeValue(index, geom[0], geom[1], myTime.cur);

          val -= computeValue(index, N);

          //printf(" computed, exact and difference \t %12.8f \t %12.8f \t %12.8f  \t %12.8f   \t %12.8f \n", vx, vx2, vy, vy2, diff);
          
          elemError += ( (val*val) * dvol );
    }//gp1
    }//gp2
 }
 else // H1 semi-norm in velocity
 {
    MatrixXd  F(2,2);
    double  v[2];

    for(gp2=0;gp2<nGP;gp2++)
    {
        param[1] = GeomData->gausspoints2[gp2];

    for(gp1=0;gp1<nGP;gp1++)
    {
          param[0] = GeomData->gausspoints1[gp1];

          GeomData->computeBasisFunctions2D(0, 2, degree, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);

          dvol = GeomData->gaussweights2[gp2] * GeomData->gaussweights1[gp1] * Jac;


          geom[0] = computeGeomOrig(0, N);
          geom[1] = computeGeomOrig(1, N);

          v[0] = analy.computeValue(0, geom[0], geom[1], myTime.cur);
          v[1] = analy.computeValue(1, geom[0], geom[1], myTime.cur);

          v[0] -= computeValue(0, N);
          v[1] -= computeValue(1, N);

          F.setZero();
          analy.computeDerivatives(geom[0], geom[1], myTime.cur, &(F(0,0)));

          F(0,0) -= computeValue(0, dN_dx);
          F(0,1) -= computeValue(0, dN_dy);
          F(1,0) -= computeValue(1, dN_dx);
          F(1,1) -= computeValue(1, dN_dy);

          val = v[0]*v[0]+v[1]*v[1] + F(0,0)*F(0,0)+F(0,1)*F(0,1)+F(1,0)*F(1,0)+F(1,1)*F(1,1);

          //printf(" computed, exact and difference \t %12.8f \t %12.8f \t %12.8f \n", dx, dy, diff);
        
          elemError += ( val * dvol );
    }//gp1
    }//gp2
 }

  return 1;
}
*/



int ElementBase::calcStiffnessAndResidual_Formulation_Displacement_2D(MatrixXd& Klocal, VectorXd& Flocal)
{
    int   err, index, ii, jj, gp, TI, TIp1, TJ, TJp1;

    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem);

    double  detF, fact, dvol, dvol0, Jac, volstrain;
    double  bb1, bb2, bb3, bb4, bb5, cc1, cc2, cc3, cc4, cc5;
    double  param[2]={0.0,0.0}, force[2]={0.0,0.0};
    double  veloCur[2]={0.0,0.0}, acceCur[2]={0.0,0.0}, sig[2]={0.0,0.0};


    int    sss   = GeomData->getModelBehaviour();
    bool   axsy  = (sss == 3);
    double thick = GeomData->getThickness();
    bool  finite = MatlData->isFiniteStrain();

    double rho0  = MatlData->getDensity() ;
    double rho   = rho0;
    
    myPoint  bforce = GeomData->getBodyForce();
    int bftf = GeomData->getBodyForceTimeFunction();
    if( bftf >= 0)
      bforce *= timeFunction[bftf]->getFactor() ;

    double af   = SolnData->td(2);
    double acceFact1 = SolnData->td(5);
    double acceFact2 = acceFact1;
    double FiniteFact = (finite == true) ? 1.0 : 0.0;


    double xNode[npElem], yNode[npElem], xx, yy;
    for(ii=0;ii<npElem;ii++)
    {
      xNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][0];
      yNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][1];
    }

    if(Klocal.rows() != nsize)
    {
      Klocal.resize(nsize, nsize);
      Flocal.resize(nsize);
    }
    Klocal.setZero();
    Flocal.setZero();

    int nGP;
    vector<double>  gausspoints1, gausspoints2, gaussweights;

    if(npElem == 3)
    {
      nGP = 1;
      getGaussPointsTriangle(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    else if(npElem == 4)
    {
      nGP = 4;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    else if(npElem == 6)
    {
      nGP = 3;
      getGaussPointsTriangle(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    else if(npElem == 9)
    {
      nGP = 9;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }



    MatrixXd  Cmat(9,9), Gc(3,9), F(3,3);
    Cmat.setZero();
    Gc.setZero();
    F.setZero();
    VectorXd  stre(9);
    stre.setZero();


    for(gp=0; gp<nGP; gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];

        GeomData->computeBasisFunctions2D(CONFIG_ORIGINAL, ELEM_SHAPE, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);

        dvol0 = gaussweights[gp]*(thick*Jac);

        xx = yy = 0.0;
        acceCur[0] = acceCur[1] = 0.0;
        for(ii=0;ii<npElem;ii++)
        {
          xx += N[ii]*xNode[ii];
          yy += N[ii]*yNode[ii];

          TI   = 2*ii;
          TIp1 = TI+1;

          acceCur[0] += (N[ii] * SolnData->acceCur[TI]);
          acceCur[1] += (N[ii] * SolnData->acceCur[TIp1]);
        }


        if(axsy)
          dvol0 *= 2.0*PI*xx;

        dvol  = dvol0;

        computeDefGrad2DCur(dN_dx, dN_dy, F);

        detF = F(0,0)*F(1,1) - F(0,1)*F(1,0);

        volstrain = F(0,0) + F(1,1) - 2.0;

        if(detF < 0.0)
        {
          throw runtime_error("Negative Jacobian in the element");
        }

        // ADJUST F33 fOR 2D PROBLEMS BASED ON THE ASSUMPTIONS OF PLANE STRESS/PLANE STRAIN/AXISYMMETRIC
        adjust_deformationgradient_2D(sss, finite, F);

        if(finite)
        {
          GeomData->computeBasisFunctions2D(CONFIG_DEFORMED, ELEM_SHAPE, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);
          dvol = gaussweights[gp]*(thick*Jac);
        }

        if(finite)  dvol *= F(2,2);

        MatlData->computeStressAndTangent(true, sss, F, F, pres, stre, Cmat, ivar, gp, myTime.dt);
        if(err !=0)    return 1;


        force[0] = rho0*bforce[0];
        force[1] = rho0*bforce[1];

        force[0] -= rho0*acceCur[0];
        force[1] -= rho0*acceCur[1];

        for(ii=0;ii<npElem;ii++)
        {
          bb1 = dvol*dN_dx[ii];
          bb2 = dvol*dN_dy[ii];
          bb4 = dvol*N[ii];
          bb5 = dvol0*N[ii];

          TI   = 2*ii;
          TIp1 = TI+1;

          Gc(0,0) = bb1 * Cmat(0,0) + bb2 * Cmat(3,0);
          Gc(0,1) = bb1 * Cmat(0,1) + bb2 * Cmat(3,1);
          Gc(0,3) = bb1 * Cmat(0,3) + bb2 * Cmat(3,3);
          Gc(0,4) = bb1 * Cmat(0,4) + bb2 * Cmat(3,4);

          Gc(1,0) = bb1 * Cmat(1,0) + bb2 * Cmat(4,0);
          Gc(1,1) = bb1 * Cmat(1,1) + bb2 * Cmat(4,1);
          Gc(1,3) = bb1 * Cmat(1,3) + bb2 * Cmat(4,3);
          Gc(1,4) = bb1 * Cmat(1,4) + bb2 * Cmat(4,4);

          sig[0] = bb1*stre[0] + bb2*stre[3];
          sig[1] = bb1*stre[1] + bb2*stre[4];

          Flocal(TI)   += (bb5*force[0] - sig[0]) ;
          Flocal(TIp1) += (bb5*force[1] - sig[1]) ;

          for(jj=0; jj<npElem; jj++)
          {
              cc1 = dN_dx[jj];
              cc2 = dN_dy[jj];
              cc5 = N[jj];

              TJ   = 2*jj;
              TJp1 = TJ+1;

              // acceleration term
              fact  = bb5*acceFact1*cc5*rho0;

              // material Stiffness contribution
              //fact  += af*(sig[0]*cc1+sig[1]*cc2)*FiniteFact;

              Klocal(TI,   TJ)    += fact ;
              Klocal(TIp1, TJp1)  += fact ;

              Klocal(TI,   TJ)    +=  af*(Gc(0,0) * cc1 + Gc(0,3) * cc2) ;
              Klocal(TI,   TJp1)  +=  af*(Gc(0,1) * cc1 + Gc(0,4) * cc2) ;
              Klocal(TIp1, TJ)    +=  af*(Gc(1,0) * cc1 + Gc(1,3) * cc2) ;
              Klocal(TIp1, TJp1)  +=  af*(Gc(1,1) * cc1 + Gc(1,4) * cc2) ;
          }
        }
    } //gp

    return 0;
}






int ElementBase::calcStiffnessAndResidual_Formulation_Displacement_3D(MatrixXd& Klocal, VectorXd& Flocal)
{
    int   err, index, ii, jj, kk, gp, TI, TIp1, TIp2, TJ, TJp1, TJp2;

    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem), dN_dz(npElem);

    double  detF, fact, dvol, dvol0, Jac, volstrain;
    double  bb1, bb2, bb3, bb4, bb5, cc1, cc2, cc3, cc4, cc5;
    double  param[3], force[3]={0.0,0.0,0.0};
    double  veloCur[3]={0.0,0.0,0.0}, acceCur[3]={0.0,0.0,0.0}, sig[3]={0.0,0.0,0.0};

    int    sss   = GeomData->getModelBehaviour();
    bool  finite = MatlData->isFiniteStrain();
    double thick = GeomData->getThickness();

    double rho0  = MatlData->getDensity() ;
    double rho   = rho0;

    myPoint  bforce = GeomData->getBodyForce();
    int bftf = GeomData->getBodyForceTimeFunction();
    if( bftf >= 0)
      bforce *= timeFunction[bftf]->getFactor() ;

    double af   = SolnData->td(2);
    double acceFact1 = SolnData->td(5);
    double acceFact2 = acceFact1;
    double FiniteFact = (finite == true) ? 1.0 : 0.0;


    double xNode[npElem], yNode[npElem], zNode[npElem], xx, yy, zz;
    for(ii=0;ii<npElem;ii++)
    {
      xNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][0];
      yNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][1];
      zNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][2];
    }

    if(Klocal.rows() != nsize)
    {
      Klocal.resize(nsize, nsize);
      Flocal.resize(nsize);
    }
    Klocal.setZero();
    Flocal.setZero();

    int nGP;
    vector<double>  gausspoints1, gausspoints2, gausspoints3, gaussweights;

    if(npElem == 4)
    {
      nGP = 1;
      getGaussPointsTetra(nGP, gausspoints1, gausspoints2, gausspoints3, gaussweights);
    }
    else if(npElem == 6)
    {
      nGP = 2;
      getGaussPointsWedge(nGP, gausspoints1, gausspoints2, gausspoints3, gaussweights);
    }
    else if(npElem == 8)
    {
      nGP = 8;
      getGaussPointsHexa(nGP, gausspoints1, gausspoints2, gausspoints3, gaussweights);
    }
    else if(npElem == 10)
    {
      nGP = 4;
      getGaussPointsTetra(nGP, gausspoints1, gausspoints2, gausspoints3, gaussweights);
    }
    else if(npElem == 18)
    {
      nGP = 9;
      getGaussPointsWedge(nGP, gausspoints1, gausspoints2, gausspoints3, gaussweights);
    }
    else if(npElem == 27)
    {
      nGP = 27;
      getGaussPointsHexa(nGP, gausspoints1, gausspoints2, gausspoints3, gaussweights);
    }



    MatrixXd  Cmat(9,9), Gc(3,9), F(3,3);
    Cmat.setZero();
    Gc.setZero();
    F.setZero();
    VectorXd  stre(9);
    stre.setZero();


    for(gp=0; gp<nGP; gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];
        param[2] = gausspoints3[gp];

        GeomData->computeBasisFunctions3D(CONFIG_ORIGINAL, ELEM_SHAPE, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), &dN_dz(0), Jac);

        dvol0 = gaussweights[gp]*Jac;

        xx = yy = zz = 0.0;
        acceCur[0] = acceCur[1] = acceCur[2] = 0.0;
        for(ii=0;ii<npElem;ii++)
        {
          xx += N[ii]*xNode[ii];
          yy += N[ii]*yNode[ii];
          zz += N[ii]*zNode[ii];

          TI   = 3*ii;
          TIp1 = TI+1;
          TIp2 = TI+2;

          acceCur[0] += (N[ii] * SolnData->acceCur[TI]);
          acceCur[1] += (N[ii] * SolnData->acceCur[TIp1]);
          acceCur[2] += (N[ii] * SolnData->acceCur[TIp2]);
        }

        dvol  = dvol0;

        computeDefGradCur(dN_dx, dN_dy, dN_dz, F);

        volstrain = F.trace() - 3.0;
        detF = F.determinant();

        if(detF < 0.0)
        {
          throw runtime_error("Negative Jacobian in the element");
        }

        if(finite)
        {
          GeomData->computeBasisFunctions3D(CONFIG_DEFORMED, ELEM_SHAPE, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), &dN_dz(0), Jac);
          dvol = gaussweights[gp]*Jac;
        }

        MatlData->computeStressAndTangent(true, sss, F, F, pres, stre, Cmat, ivar, gp, myTime.dt);
        if(err !=0)    return 1;
        //printMatrix(Cmat);


        force[0] = rho0*bforce[0];
        force[1] = rho0*bforce[1];
        force[2] = rho0*bforce[2];

        force[0] -= rho0*acceCur[0];
        force[1] -= rho0*acceCur[1];
        force[2] -= rho0*acceCur[2];

        for(ii=0;ii<npElem;ii++)
        {
          bb1 = dvol*dN_dx[ii];
          bb2 = dvol*dN_dy[ii];
          bb3 = dvol*dN_dz[ii];
          bb4 = dvol*N[ii];
          bb5 = dvol0*N[ii];

          for(kk=0; kk<9; kk++)
          {
            Gc(0,kk) = bb1 * Cmat(0,kk) + bb2 * Cmat(3,kk) + bb3 * Cmat(6,kk);
            Gc(1,kk) = bb1 * Cmat(1,kk) + bb2 * Cmat(4,kk) + bb3 * Cmat(7,kk);
            Gc(2,kk) = bb1 * Cmat(2,kk) + bb2 * Cmat(5,kk) + bb3 * Cmat(8,kk);
          }

          TI   = 3*ii;
          TIp1 = TI+1;
          TIp2 = TI+2;

          sig[0] = bb1*stre[0] + bb2*stre[3] + bb3*stre[6] ;
          sig[1] = bb1*stre[1] + bb2*stre[4] + bb3*stre[7] ;
          sig[2] = bb1*stre[2] + bb2*stre[5] + bb3*stre[8] ;

          Flocal(TI)   += (bb5*force[0] - sig[0]) ;
          Flocal(TIp1) += (bb5*force[1] - sig[1]) ;
          Flocal(TIp2) += (bb5*force[2] - sig[2]) ;

          for(jj=0; jj<npElem; jj++)
          {
            cc1 = dN_dx[jj];
            cc2 = dN_dy[jj];
            cc3 = dN_dz[jj];
            cc5 = N[jj];

            TJ   = 3*jj;
            TJp1 = TJ+1;
            TJp2 = TJ+2;

            // acceleration term
            fact  = bb5*acceFact1*cc5*rho0;

            // material Stiffness contribution
            //fact += af*(sig[0]*cc1 + sig[1]*cc2 + sig[2]*cc3)*FiniteFact;

            Klocal(TI,   TJ)    +=  fact;
            Klocal(TIp1, TJp1)  +=  fact;
            Klocal(TIp2, TJp2)  +=  fact;

            Klocal(TI,   TJ)    +=  af*(Gc(0,0) * cc1 + Gc(0,3) * cc2 + Gc(0,6) * cc3) ;
            Klocal(TI,   TJp1)  +=  af*(Gc(0,1) * cc1 + Gc(0,4) * cc2 + Gc(0,7) * cc3) ;
            Klocal(TI,   TJp2)  +=  af*(Gc(0,2) * cc1 + Gc(0,5) * cc2 + Gc(0,8) * cc3) ;

            Klocal(TIp1, TJ)    +=  af*(Gc(1,0) * cc1 + Gc(1,3) * cc2 + Gc(1,6) * cc3) ;
            Klocal(TIp1, TJp1)  +=  af*(Gc(1,1) * cc1 + Gc(1,4) * cc2 + Gc(1,7) * cc3) ;
            Klocal(TIp1, TJp2)  +=  af*(Gc(1,2) * cc1 + Gc(1,5) * cc2 + Gc(1,8) * cc3) ;

            Klocal(TIp2, TJ)    +=  af*(Gc(2,0) * cc1 + Gc(2,3) * cc2 + Gc(2,6) * cc3) ;
            Klocal(TIp2, TJp1)  +=  af*(Gc(2,1) * cc1 + Gc(2,4) * cc2 + Gc(2,7) * cc3) ;
            Klocal(TIp2, TJp2)  +=  af*(Gc(2,2) * cc1 + Gc(2,5) * cc2 + Gc(2,8) * cc3) ;
          }
        }
    } //gp
    
    //printMatrix(Klocal);    printVector(Flocal);

    return 0;
}

